%author: wxj233
%time: 2024.1.31 20:34
%function:  粒子滤波实验

clc;
clear;
clear Function;
close all;

% 3倍sigma点就是精度
p_d = 0.4/3;  % 测距精度0.4m
p_v = 0.02778/3;  % 测速精度0.0277m/s
p_theta = 0.3/180*pi/3; % 测角精度0.3°（近距离），远距离是0.1°。都是波束中心附近

% 初始化仿真器
% dt
dt = 0.072;
simulater = Simulater(dt);

% 产生第三个目标特征点群
% len, width, probabilitys, varargin{seed}
cluster3 = simulater.generateFeaturePoints(5, 2.2, 1, 4);
% % 根据特征点群，产生第三条轨迹
% cluster, point0, v, a, T0, Times, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
[T3_1, rT, ep3_1, ev3_1, et3_1] = simulater.generateTrack(cluster3, [-150, 60], [10, 0], [0, 0], 0, [0, 30], 3, p_d, p_v, p_theta, 71, 72);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]

% 引入噪声, 根据区域大小和噪声密度来算噪声数量，且数量服从泊松分布, 在区域内服从均匀分布，区域自己根据轨迹范围设定
% density varargin:{seed}
[noise, area] = simulater.generateNoise(0, 1); % [[t, r, theta, vr, x, y, vx, vy, id]; ...]

[frame, isNextFrame]= simulater.getNextFrame();
frames = [];

count = 1;
while isNextFrame
    frames = [frames; frame];  % [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]

    


    [frame, isNextFrame]= simulater.getNextFrame();
    count = count+1;
end


